#include "headfile.h"

const float MidK[] = {
	0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 5.3636, 4.9167, 4.5385, 4.2143, 4.2143, 3.9333, 3.6875, 3.4706, 3.1053, 2.9500, 2.8095, 2.6818, 2.5652, 2.4583, 2.3600, 2.2692, 2.1852, 2.1071, 2.0345, 1.9667, 1.9032, 1.8438, 1.7879, 1.7353, 1.6857, 1.6389, 1.5946, 1.5526, 1.5128, 1.4750, 1.4390, 1.4048, 1.3721, 1.3409, 1.3111, 1.2826, 1.2553, 1.2292, 1.2041, 1.1800, 1.1569, 1.1346, 1.1346, 1.1132, 1.0926, 1.0727, 1.0536, 1.0351, 1.0172, 1.0000, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344, 0.6344,
};

const int DistToPro[] = {
	68,59,53,49,45,43,40,38,36,34,32,31,30,29,28,28,27,27,26,26,26,25 };//最低行为10cm 最远行为65cm



//-------------------------------------------------------------------------------------------------------------------
//  @brief      选择速度模式、期望大小
//  @param      unsigned char 速度类型type
//  @return     void	
//  @note            
//-------------------------------------------------------------------------------------------------------------------
void ChangeSpeedFlag(unsigned char type)
{
#define BASICLINE 42		//加减速基准行
	if (1 == type)
	{
		//speed_type = 1;

		if (Img_RampFlag) spdExp1 = spdBas1;
		else if (SpeedRow <= UP_EDGE + 1) spdExp1 = spdBas1 + 6;
		else if (SpeedRow > BASICLINE) spdExp1 = spdBas1 + (BASICLINE - SpeedRow) * speed_k1;
		else spdExp1 = spdBas1 + (BASICLINE - SpeedRow) * (BASICLINE - SpeedRow) * speed_k2;  //最大提3线
		if (CircleState >= 2 && spdExp1 > spdBas1) spdExp1 = spdBas1;  //环岛限速
	}
	// else if (0 == type)
	// {
	// 	speed_type = 4;
	// }
	else if (2 == type)		//纯电磁速控
	{
		speed_type = 2;
		if (SpeedRow > BASICLINE) spdExp2 = spdBas2;
		else spdExp2 = spdBas2 + (BASICLINE - SpeedRow) * (BASICLINE - SpeedRow) * 0.024f;
	}    
	else;
        
        if(test_flag)
          speed_type=3;
}

//-------------------------------------------------------------------------------------------------------------------
//  @brief      舵机控制
//  @param      void
//  @return     void	
//  @note            
//-------------------------------------------------------------------------------------------------------------------
void SteerControl(void)
{
	if (g_steer_open)
//		if(!GarageOut_Flag&&!Garage_In_Open)
			pwm_duty(STEER_PWMCH,SteerDuty);
    else
        pwm_duty(STEER_PWMCH,SteerDutySet);
}

//-------------------------------------------------------------------------------------------------------------------
//  @brief      摄像头误差获取
//  @param      void
//  @return     void	
//  @note            
//-------------------------------------------------------------------------------------------------------------------
void CameraSteerControl(void)
{
	//获取误差
	if (ProSpect < ML_Count)
	{
		Error_1 = (ControlMid - ML[ML_Count]);
	}
	else
	{
		Error_1 = (ControlMid - ML[ProSpect]);
	}
	//if (Error_1 > 20 || Error_1 < -20)
	//{
	//	//加权平均
	//	int end_line = (91 - 10) / 5;
	//	for (int i = end_line; i > 0; i--)
	//		if (DistToPro[i] > ML_Count)
	//		{
	//			end_line = i;
	//			break;
	//		}
	//	float sum_err = 0, sum_ave = 0;
	//	for (int i = DOWN_EAGE; i > DistToPro[end_line]; i--)
	//	{
	//		sum_err += (ControlMid - ML[i]) * MidK[i] * MidK[i];
	//		sum_ave += MidK[i];
	//	}
	//	Error_1 = (int)(0.5 + sum_err / sum_ave);
	//}
	//校验误差
	if (ErrorFlag)
		Error_1 = Error_2;
	//舵机输出计算
	float P = 0, D = 0;
	//P值改变
	  //动态p
	float k = (p_max - p_min) / 94;
	if (Error_1 > 0)
	{
		Steer_P = p_max - k * Error_1;
	}
	else
	{
		Steer_P = p_max + k * Error_1;
	}
	if (CircleFlag)
	{
		P = Steer_P_CI;
	}
	else
	{
		P = Steer_P;
	}
	//出入弯D
	if (Error_1 > 0 && Error_1 - Error_2 > 0
		|| Error_1 < 0 && Error_1 - Error_2 < 0)  //入弯
		D = Steer_D;
	else D = 0;

	SteerDuty = INITDUTY + (int)(P * Error_1 + D * (Error_1 - Error_2) + 0.5);
	Error_2 = Error_1;
	SteerDuty = MAX(SteerDuty, INITDUTY - 690);//限幅注意一下
	SteerDuty = MIN(SteerDuty, INITDUTY + 700);
}

//-------------------------------------------------------------------------------------------------------------------
//  @brief      舵机控制
//  @param      void
//  @return     void	
//  @note            
//-------------------------------------------------------------------------------------------------------------------
void CarControl(void)
{
	// if (Img_BlockFlag || g_GetMeetingFlag || g_StateMaster >= StateStop)        //路障控制车辆 或者 会车区
	// {
	// 	return;
	// }
//	else								//正常情况
	//{
//		if (!Img_BlockFlag && 2 != Img_BrokenFlag && g_StateMaster < StateGo)
//			Ind_StopCar();//电磁停车
		// if (!g_camera_open && g_ind_open
		// 	|| g_camera_open && (2 == Img_BrokenFlag || 3 == Img_BrokenFlag)
		// 	|| g_camera_open && (1 == Img_BrokenFlag || 4 == Img_BrokenFlag || 5 == Img_BrokenFlag) && ML_Count > 50)
		// {
		// 	ChangeSpeedFlag(0);
		// 	IndSteerControl();
		// }
		// else if (g_camera_open && g_handle_open)
		// {
		// 	ChangeSpeedFlag(1);
		// 	CameraSteerControl();
		// }
		// else g_drive_flag = 0;
		// SteerControl();//舵机控制
//	}
    if(g_camera_open && g_handle_open)
    {
        ChangeSpeedFlag(1);
        CameraSteerControl();
    }
    else
        g_drive_flag=0;
    SteerControl();
}

